#include <Arduino.h>
#include <WiFi.h>
#include "esp_camera.h"
#include "UdpClient.hpp"

const char *ssid = WIFI_SSID;
const char *passwd = WIFI_PASSWORD;
const char *host = SERVER_HOST;
const uint16_t serverUdpPort = 8004;
/*
   获取通道索引的端口(tcp)
   当然也可以使用发送图片的UDP客户端获取，但必须保证获取到之后才能发送图片帧，需要反复判断重传和双向ack
   故在启动时通过TCP获取通道索引
  */
const uint16_t channelBindPort = 8004;
const uint16_t localUdpPort = 2333;

LightUDP streamSender;
// 绑定sn后续为mac 作为sn  代表后续内容是SN 后一位是 6表示SN长度 空位的0 将填充mac

int channel_index = -1;


void connectWifi(const char *ssid, const char *passphrase) {
    WiFi.mode(WIFI_STA);
    WiFi.begin(ssid, passphrase);

    log_i("connecting to router... ");
    //等待wifi连接成功
    while (WiFi.status() != WL_CONNECTED) {
        log_printf(".");
        delay(500);
    }
    const IPAddress &localIp = WiFi.localIP();
    const IPAddress &gatewayIp = WiFi.gatewayIP();
    log_i("\nWiFi connected, local IP address:%u.%u.%u.%u\nWifi GWip is:%u.%u.%u.%u\n",
          localIp[0], localIp[1], localIp[2], localIp[3],
          gatewayIp[0], gatewayIp[1], gatewayIp[2], gatewayIp[3]);
}

void setup() {
    camera_config_t config;
    config.ledc_channel = LEDC_CHANNEL_0;
    config.ledc_timer = LEDC_TIMER_0;
    config.pin_d0 = Y2_GPIO_NUM;
    config.pin_d1 = Y3_GPIO_NUM;
    config.pin_d2 = Y4_GPIO_NUM;
    config.pin_d3 = Y5_GPIO_NUM;
    config.pin_d4 = Y6_GPIO_NUM;
    config.pin_d5 = Y7_GPIO_NUM;
    config.pin_d6 = Y8_GPIO_NUM;
    config.pin_d7 = Y9_GPIO_NUM;
    config.pin_xclk = XCLK_GPIO_NUM;
    config.pin_pclk = PCLK_GPIO_NUM;
    config.pin_vsync = VSYNC_GPIO_NUM;
    config.pin_href = HREF_GPIO_NUM;
    config.pin_sccb_sda = SIOD_GPIO_NUM;
    config.pin_sccb_scl = SIOC_GPIO_NUM;
    config.pin_pwdn = PWDN_GPIO_NUM;
    config.pin_reset = RESET_GPIO_NUM;
    config.xclk_freq_hz = 20000000;
    config.pixel_format = PIXFORMAT_JPEG;
    config.grab_mode = CAMERA_GRAB_WHEN_EMPTY;
    config.fb_location = CAMERA_FB_IN_PSRAM;
    config.jpeg_quality = 12;
    config.fb_count = 1;

    if (psramFound()) {
        config.frame_size = FRAMESIZE_UXGA;
        config.jpeg_quality = 10;
        config.fb_count = 2;
        config.grab_mode = CAMERA_GRAB_LATEST;
    } else {
        // 使用直接内存申请
        config.frame_size = FRAMESIZE_SVGA;
        config.fb_location = CAMERA_FB_IN_DRAM;
    }


    // camera init
    esp_err_t err = esp_camera_init(&config);
    if (err != ESP_OK) {
        log_i("Camera init failed with error 0x%x", err);
        return;
    }
    log_i("get sensor ");
    sensor_t *s = esp_camera_sensor_get();
    s->set_framesize(s, FRAMESIZE_HVGA);

    connectWifi(ssid, passwd);
    WiFiClient client;

    uint8_t bind_sn[] = {'S', 'N', ':', 6, 0, 0, 0, 0, 0, 0};
    // 将mac设置到 bind_sn 第3位
    WiFi.macAddress(&bind_sn[4]);
    log_i("MAC is:%02X%02X%02X%02X%02X%02X", bind_sn[4], bind_sn[5],
                  bind_sn[6], bind_sn[7], bind_sn[8], bind_sn[9]);
    int index;
    do {
        while (!client.connect(host, channelBindPort)) {
            log_i("connection failed,wait 3 sec...");
            delay(3000);
        }
        client.setNoDelay(true);
        client.write(bind_sn, 10);
        client.flush();
        delay(3000);
        index = client.read();
    } while (index == -1);

    if (index <= 255) {
        channel_index = index;
        log_i("channel_index is : %d ", channel_index);
    }
    client.stop();

    streamSender.begin(WiFi.localIP(), localUdpPort);
    streamSender.setServer(host, serverUdpPort);
}

void loop() {
    log_i("do loop");
    if (channel_index == -1) {
        log_i("delay 3 sec");
        delay(3000);
        return;
    }
    camera_fb_t *fb = NULL;
    size_t len;
    log_i("send image");

    while (true) {
        fb = esp_camera_fb_get();
        if (!fb) {
            log_i("Camera capture failed");
            break;
        }
        len = fb->len;
        fb->buf[len] = channel_index;
        streamSender.send(fb->buf, len + 1);
        esp_camera_fb_return(fb);
    }
}